#include "reflect_detector.h"

Reflector::ReflectorDetector::ReflectorDetector(): Node("reflector_detector") 
{
    // 声明节点参数
    this->declare_parameter("reflector_min_instensity", Reflector::REFLECTOR_MIN_INTENSITY);
    this->declare_parameter("reflector_radius", Reflector::REFLECTOR_RADIUS);
    this->declare_parameter("reflector_points_num", Reflector::REFLECTOR_POINT_NUM);
    this->declare_parameter("reflector_min_distance", Reflector::REFLECTOR_MIN_DISTANCE);
    this->declare_parameter("reflector_max_distance", Reflector::REFLECTOR_MAX_DISTANCE);
    this->declare_parameter("reflector_match_thres", Reflector::REFLECTOR_MATCH_THRES);

    this->next_id = 0;
    this->pre_positions = {
        {0, 0};
        {0, 1};
        {3, 0};
    }
    // 订阅激光雷达数据
    laser_sub_ = this->create_subscription<sensor_msgs::msgs::LaserScan>(
        "scan", 10, std::bind(&Reflector::ReflectorDetector::LaserCallback, this, std::placeholders::_1)
    );

    // 发布 landmark 话题
    landmark_pub_ = this->create_publisher<cartographer_ros_msgs::LandmarkList> (
        "landmark", 10
    );
} 

std::set<double> Reflector::ReflectorDetector::GetDistanceSignature(std::pair<double, double>& point,
                                                                    std::vector<std::pair<double, double>>& points)
{
    auto min_distance = this->getparameter("reflector_min_distance").as_double();
    auto max_distance = this->getparameter("reflector_max_distance").as_double();

    std::set<double> signature;

    for (auto& other: points) 
    {
        double dist = hypot(point.first -  other.first,
                            point.second -  other.second);

        if (dist > min_distance && 
            dist < max_distance)
        {
            signature.insert(dist);
        }
    }

    return signature;
}

double Reflector::ReflectorDetector::GeMatchSignature( std::set<double> & sg1,
                                                       std::set<double> & sg2)
{
    if (sig1.empty() || sig2.empty())
        return std::numeric_limits<double>::max;
    
    auto it1 = sg1.beign();
    auto it2 = sg2.beign();
    double diff = 0;
    int count = 0;

    while (it1 != sig1.end() && 
           it2 != sig2.end())
    {
        diff += std::abs(*it1 - *it2);
        count++;
        ++it1;
        ++it2;
    }

    return count > 0 ? diff / count :std::numeric_limits<double>::max;
}

void Reflector::ReflectorDetector::AssignStableIds(std::vector<std::pair<double, double>>& current_positions)
{
    auto match_thres = this->getparameter("reflector_match_thres").as_double();

    // 计算每个反光柱的距离列表
    std::vector<std::set<double>> current_signatures;
    std::vector<std::set<double>> prev_signatures;
    for (auto &pos: current_positions)
    {
        current_signatures.push_back(GetDistanceSignature(pos, current_positions));
    }
    for (auto &pos: pre_positions)
    {
        current_signatures.push_back(GetDistanceSignature(pos, pre_positions));
    }

    // 清空 landmark_list
    landmark_list.clear();
    next_id = 0;

    // 计算和预先设定好的距离列表的匹配度
    for(int c_idx = 0; c_idx < current_landmarks.size(); c_idx++)
    {
        std::min_diff = std::numeric_limits<double>::max();
        int best_match_id = -1;

        for (auto & pre_sig: pre_signatures)
        {
            double diff = GeMatchSignature(current_signatures[c_idx],pre_sig);

            // 寻找匹配度最好的点
            if (diff < match_thres && diff < min_diff)
            {
                min_diff = diff;
                best_match_id = c_idx;
            }
        }

        // 如果找到了匹配的反光柱
        if (best_match_id != -1)
        {
            std::string new_id = std::to_string(next_id++);
            ReflectorPost new_reflector {
                new_id,
                current_positions[c_idx].first,
                current_positions[c_idx].second,
                current_signatures[c_idx]
            };

            // 按照 landmark 的格式要求创建单个 landmark 条目
            cartographer_ros_msgs::LandmarkEntry landmark_entry;
            // 地标编号和位姿
            landmark_entry.id = new_reflector.index;
            landmark_entry.tracking_from_landmark_transform.position.x = new_reflector.x;
            landmark_entry.tracking_from_landmark_transform.position.y = new_reflector.y;
            landmark_entry.tracking_from_landmark_transform.position.z = 0;   

            landmark_entry.tracking_from_landmark_transform.orientation.w = 1.0;
            // 设置landmark权重
            landmark_entry.translation_weight = 20.0;
            landmark_entry.rotation_weight = 20.0;

            landmark_list.push_back(landmark_entry);
        }
    } 
}

void Reflector::ReflectorDetector::LaserCallback(const sensor_msgs::msg::LaserScan::SharedPtr scan)
{
    // 获取节点参数
    auto min_intensity = this->get_parameter("reflector_min_instensity").as_double();
    auto radius = this->get_parameter("reflector_radius").as_double();
    auto min_num = this->get_parameter("reflector_points_num").as_double();

    // 雷达数据候选点
    std::vector<Reflect::LaserData> candidates;

    // 首先进行筛选得出候选的反光柱点
    for (int i = 0; i < scan->ranges.size(); i++)
    {
        // 跳过不符合要求的点
        if (scan->intensities[i] < min_intensity || // 激光强度不符合要求
            std::isnan(scan->ranges[i]) || // 距离为无穷
            scan->ranges[i] < scan->range_min ||
            scan->ranges[i] > scan->range_max)
        {
            continue;
        }

        // 从极坐标系转换至笛卡尔坐标系
        float angle = scan->angle_min + i * scan->angle_increment; // 当前的极角为 angle_min + 角度增量


        Reflector::LaserData s = {
            static_cast<int>(i), // 索引号转为int类型
            scan->ranges[i],
            scan->intensities[i],
            scan->ranges[i] * cos(angle);
            scan->ranges[i] * sin(angle);
        };

        candidates.push_back(s);
    }

    // 聚类识别，放光柱为一圈激光强度较大的聚类
    std::vector<Reflect::LaserData> clusters;
    if (!candidates.empty())
    {
        // 用候选的第一个点初始化当前聚类
        std::vector<Reflect::LaserData> current_cluster = {candidates[0]};

        // 从第二个点开始计算欧氏距离
        for (int i = 1; i < candidates.size(); i++)
        {
            double dist = hypot(candidates[i].x -  candidates[i-1].x,
                                candidates[i].y -  candidates[i-1].y);

            // 若距离小直径，则属于同一个聚类
            if (dist < radius * 2)
            {
                current_cluster.push_back(candidates[i]);
            }
            else
            {
                // 如果聚类的点数大于阈值，则记录该聚类
                if (current_cluster.size() > min_num)
                {
                    clusters.push_back(current_cluster);
                }

                // 以当前的候选点为新的聚类起点，开始新的聚类搜索
                current_cluster = {candidates[i]};
            }

            // 跳出循环后，处理最后的聚类
            if (current_cluster.size() > min_num)
            {
                clusters.push_back(current_cluster);
            }
        }

        // 每个聚类都是一个反光柱，直接记录聚类的中心坐标就是反光柱的坐标
        // 计算完坐标后，按照landmark的消息格式进行发布
        // 由于是每次订阅雷达数据后就发布landmark，因此设置二者的时间戳和帧相同
        landmark_list.header = scan->header;

        for (auto& cluster: clusters)
        {
            double cnt_x = 0;
            double cnt_y = 0;

            for(auto& candidate: cluster)
            {
                cnt_x += candidate.x;
                cnt_y += candidate.y;
            }

            cnt_x /= cluster.size();
            cnt_y /= cluster.size();
        }

        AssignStableIds();

        landmark_pub_->publish(this->landmarks_list);
        RCLCPP_DEBUG(this->get_logger(), "landmark %s at (%.2f, %.2f)", 
            landmark_list.id, landmark_list.x,landmark_list.y);
    }
}